#include <math.h>
#include "simpleroboticmath.h"

void d4Matrix2EulerZYX(double Matrix[4][4],double (&PosXYZ)[3], double (&EulerZYX)[3]){

    double B = atan2(-Matrix[2][0],sqrt(Matrix[0][0]*Matrix[0][0]+Matrix[1][0]*Matrix[1][0]));
    double A = atan2(Matrix[1][0]/cos(B),Matrix[0][0]/cos(B));
    double C = atan2(Matrix[2][1]/cos(B),Matrix[2][2]/cos(B));

    PosXYZ[0] = Matrix[0][3];
    PosXYZ[1] = Matrix[1][3];
    PosXYZ[2] = Matrix[2][3];

    EulerZYX[0] =A * 180 / 3.1415;
    EulerZYX[1] =B * 180 / 3.1415;
    EulerZYX[2] =C * 180 / 3.1415;
}

void EulerZYX2d4Matrix(double PosXYZ[3],double EulerZYX[3],double (&Matrix)[4][4]){

    double a = EulerZYX[0] * 3.1415 / 180;
    double b = EulerZYX[1] * 3.1415 / 180;
    double c = EulerZYX[2] * 3.1415 / 180;

    Matrix[0][0] = cos(a)*cos(b); Matrix[0][1] = cos(a)*sin(b)*sin(c)-sin(a)*cos(c);  Matrix[0][2] = cos(a)*sin(b)*cos(c)+sin(a)*sin(c);
    Matrix[1][0] = sin(a)*cos(b); Matrix[1][1] = sin(a)*sin(b)*sin(c)+cos(a)*cos(c);  Matrix[1][2] = sin(a)*sin(b)*cos(c)-cos(a)*sin(c);
    Matrix[2][0] = -sin(b);       Matrix[2][1] = cos(b)*sin(c);                       Matrix[2][2] = cos(b)*cos(c);
    Matrix[3][0] = 0;             Matrix[3][1] = 0;                                   Matrix[3][2] = 0;

    Matrix[0][3] = PosXYZ[0];
    Matrix[1][3] = PosXYZ[1];
    Matrix[2][3] = PosXYZ[2];
    Matrix[3][3] = 1;
}

void d4MatrixInverse(double Matrix[4][4],double (&InerseMatrix)[4][4]){
    int i = 0;
    int j = 0;
    double MatrixAOrientationTranspose[3][3];
    for(i = 0; i < 3; i++){
        for(j = 0; j < 3; j++){
            InerseMatrix[i][j] = Matrix[j][i];
            MatrixAOrientationTranspose[i][j] = -Matrix[j][i];
        }
    }

    InerseMatrix[0][3] = MatrixAOrientationTranspose[0][0]*Matrix[0][3] + MatrixAOrientationTranspose[0][1]*Matrix[1][3] + MatrixAOrientationTranspose[0][2]*Matrix[2][3];
    InerseMatrix[1][3] = MatrixAOrientationTranspose[1][0]*Matrix[0][3] + MatrixAOrientationTranspose[1][1]*Matrix[1][3] + MatrixAOrientationTranspose[1][2]*Matrix[2][3];
    InerseMatrix[2][3] = MatrixAOrientationTranspose[2][0]*Matrix[0][3] + MatrixAOrientationTranspose[2][1]*Matrix[1][3] + MatrixAOrientationTranspose[2][2]*Matrix[2][3];

    InerseMatrix[3][0] = 0;
    InerseMatrix[3][1] = 0;
    InerseMatrix[3][2] = 0;
    InerseMatrix[3][3] = 1;
}

void d4MatrixMultiply(double MatrixA[4][4],double MatrixB[4][4],double (&MultiplyMatrix)[4][4]){
    int i = 0;
    int j = 0;
    int k = 0;

    for(i = 0; i < 4; i++){
        for(j = 0; j < 4; j++){
            MultiplyMatrix[i][j] = 0;
        }
    }

    for(i = 0; i < 4; i++){
        for(j = 0; j < 4; j++){
            for(k = 0; k < 4; k++){
                MultiplyMatrix[i][j] = MultiplyMatrix[i][j] + (MatrixA[i][k] * MatrixB[k][j]);
            }
        }
    }
}

void d4MatrixPlus(double MatrixA[4][4],double MatrixB[4][4],double (&PlusMatrix)[4][4]){
    int i = 0;
    int j = 0;
    for(i = 0; i < 4; i++){
        for(j = 0; j < 4; j++){
            PlusMatrix[i][j] = MatrixA[i][j] + MatrixB[i][j];
        }
    }
}
